﻿#include <learn/alg/alg_equation.h>
#include <learn/math/differential.h>
#include <learn/math/matrix_solver.h>

using namespace xi;
using namespace xi::alg;

double Alg_RootOfEquation::solve(Func11 f, double x)
{
    static constexpr double h = Numeric::distance;
    double dx = 0;
    for (std::size_t i = 0; i < Numeric::iter; i++)
    {
        double fx = f(x);
        double j = (f(x + h) - f(x - h)) / (2 * h);

        // 迭代推进 x = x - f / f'
        dx = fx / j;
        x = x - dx;

        // 当变化量足够小或者是根就退出
        if (std::min(std::abs(dx), std::abs(fx)) < Numeric::distance)
            break;
    }

    return x;
}

VectorXd Alg_RootOfEquation::solve(FuncXX f, VectorXd x)
{
    Vector dx(x.size());
    for (std::size_t i = 0; i < Numeric::iter; i++)
    {
        // 获得 Jacobi 矩阵
        MatrixXd J = jacobian(f, x);
        if (J.norm() < std::numeric_limits<double>::epsilon())
            break;

        // JT*Jdx = -JT*f
        VectorXd fx = f(x).transpose() * J; // 变为行向量
        MatrixXd JTJ = J.transposed() * J;

        Matrix_PTLU_Solver solver(JTJ);
        solver.perform();
        dx = solver.solve(std::move(fx)).transpose();
        x -= dx;

        // 当变化量足够小或者是根就退出
        if (std::min(dx.norm(), fx.norm()) < Numeric::distance)
            break;
    }

    return x;
}